package com.yuneec.droneservice.command;

import com.yuneec.droneservice.parse.DroneFeedback;
import com.yuneec.droneservice.request.IResponse;
import com.yuneec.droneservice.utils.ByteUtils;

import java.util.Arrays;

/**
 * Created by gjw on 2017/3/27.
 */

public class GetTelemetryData extends BaseCommand {

    public GetTelemetryData() {
        super(MAV_RC_COMMAND.MAV_RC_MSG_TELEMETRY_DATA);
    }

    @Override
    public IResponse<DroneFeedback> toResponse(byte[] data, int off, int len) {
        int code = data[off + 1] & 0xff;
        IResponse<DroneFeedback> response = new IResponse(ids(), code);
        if (code == MAV_RC_ERROR_CODE.MAV_RC_ERR_SUCCESS) {
            DroneFeedback droneFeedback = new DroneFeedback();
            droneFeedback.latitude = ByteUtils.byteArrayToInt(Arrays.copyOfRange(data, off + 4, off + 8), IS_BIGENDIAN) * 1e-7f;
            droneFeedback.longitude = ByteUtils.byteArrayToInt(Arrays.copyOfRange(data, off + 8, off + 12), IS_BIGENDIAN) * 1e-7f;
            droneFeedback.altitude = ByteUtils.byteArrayToInt(Arrays.copyOfRange(data, off + 12, off + 16), IS_BIGENDIAN) * 0.01f;
            float vx = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 16, off + 18), IS_BIGENDIAN) * 0.01f;
            float vy = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 18, off + 20), IS_BIGENDIAN) * 0.01f;
            float vz = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 20, off + 22), IS_BIGENDIAN) * 0.01f;
            droneFeedback.tas = (float) Math.sqrt(vx * vx + vy * vy + vz * vz);
            droneFeedback.vSpeed = vz;
            droneFeedback.hSpeed = (float) Math.sqrt(vx * vx + vy * vy);
            droneFeedback.gpsUsed = ((data[off + 22] & 0x80) >> 7) > 0;
            droneFeedback.fixType = ((data[off + 22] & 0x60)) >> 5;
            droneFeedback.satellitesNum = data[off + 22] & 0x1f;
            droneFeedback.voltage = (data[off + 23] & 0xff) * 0.1f + 5;
            droneFeedback.current = (data[off + 24] & 0xff) * 0.5f;
            droneFeedback.roll = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 25, off + 27), IS_BIGENDIAN) * 0.01f;
            droneFeedback.pitch = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 27, off + 29), IS_BIGENDIAN) * 0.01f;
//        droneFeedback.yaw = normalizeYaw(ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, 29, 31), IS_BIGENDIAN) * 0.01f);
            droneFeedback.yaw = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 29, off + 31), IS_BIGENDIAN) * 0.01f;
            droneFeedback.motorStatus = data[off + 31] & 0xff;
            droneFeedback.imuStatus = data[off + 32] & 0xff;
//        droneFeedback.gpsStatus = (data[32] & 0x20) >> 5;
            droneFeedback.pressCompassStatus = data[off + 33] & 0xff;
            droneFeedback.fMode = data[off + 34] & 0xff & 0x7f;
            droneFeedback.vehicleType = data[off + 35] & 0xff;
//        droneFeedback.cgpsStatus = droneFeedback.imuStatus >> 6;
//        droneFeedback.gpsPosUsed = (droneFeedback.fMode >> 7) > 0;
//            }
            droneFeedback.errorFlags1 = data[off + 36] & 0xff;
            droneFeedback.gpsAccH = (data[off + 37] & 0xff) * 0.05f;
            droneFeedback.fskRssi = data[off + 38];
            response.setParam(droneFeedback);
        }
        return response;
    }


    /**
     * @param yaw original yaw ,may be negative
     * @return yaw between [0,360]
     */
    public static float normalizeYaw(float yaw) {
        float rtn = 0;

        if (yaw < 0) {
            yaw += 360;
        }

        if (yaw >= 0 && yaw <= 90) {
            rtn = 90 - yaw;
        } else if (yaw > 90 && yaw <= 360) {
            rtn = 450 - yaw;
        }
        return rtn;
    }
}
